#!/usr/bin/python3
#coding:utf-8
import rospy
import asyncio
from mavsdk import System
from mavsdk.offboard import (OffboardError, VelocityNedYaw)

class MavTest:
    def __init__(self):
        rospy.init_node('mav_test_node', anonymous = True)
        self.rate = rospy.Rate(20)
        self.pkg_path = rospy.get_param("/pkg_path/mav_test")
        self.serial_path = rospy.get_param("/serial_path")

    async def start_offboard(self):
        print("-- Arming")
        await self.drone.action.arm()

        print("-- Setting initial setpoint")
        await self.drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 0.0, 0.0, 0.0))

        print("-- Starting offboard")
        try:
            await self.drone.offboard.start()
        except OffboardError as error:
            print(f"Starting offboard mode failed with error code: \
                {error._result.result}")
            print("-- Disarming")
            await self.drone.action.disarm()
            return

        print("-- Go up 2 m/s")
        await self.drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 0.0, -2.0, 0.0))
        await asyncio.sleep(4)

        print("-- Go North 2 m/s, turn to face East")
        await self.drone.offboard.set_velocity_ned(VelocityNedYaw(2.0, 0.0, 0.0, 90.0))
        await asyncio.sleep(4)

        print("-- Go South 2 m/s, turn to face West")
        await self.drone.offboard.set_velocity_ned(
            VelocityNedYaw(-2.0, 0.0, 0.0, 270.0))
        await asyncio.sleep(4)

        print("-- Go West 2 m/s, turn to face East")
        await self.drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, -2.0, 0.0, 90.0))
        await asyncio.sleep(4)

        print("-- Go East 2 m/s")
        await self.drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 2.0, 0.0, 90.0))
        await asyncio.sleep(4)

        print("-- Turn to face South")
        await self.drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 0.0, 0.0, 180.0))
        await asyncio.sleep(2)

        print("-- Go down 1 m/s, turn to face North")
        await self.drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 0.0, 1.0, 0.0))
        await asyncio.sleep(4)

        print("-- Stopping offboard")
        try:
            await self.drone.offboard.stop()
        except OffboardError as error:
            print(f"Stopping offboard mode failed with error code: \
                {error._result.result}")

    async def run(self):

        self.drone = System()
        await self.drone.connect(system_address=self.serial_path)

        print("Waiting for drone to connect...")
        async for state in self.drone.core.connection_state():
            if state.is_connected:
                print(f"Drone discovered!")
                break

        print("Waiting for drone to have a global position estimate...")
        async for health in self.drone.telemetry.health():
            if health.is_global_position_ok:
                print("Global position estimate ok")
                break

        print("-- Arming")
        await self.drone.action.arm()

        print("-- Taking off")
        await self.drone.action.takeoff()
        await asyncio.sleep(5)

        print("-- Landing")
        await self.drone.action.land()

    def MainLoop(self):
        while not rospy.is_shutdown():
            self.rate.sleep()

if __name__ == "__main__":
    loop = asyncio.get_event_loop()
    temp_mav_test = MavTest()
    loop.run_until_complete(temp_mav_test.run())
    temp_mav_test.MainLoop()
